/*
Copyright(C) 2021 Xiyin,li in hust.
All Rights Reserved.
This file is a node which can receive a package from serial
and unpack the data then publish a topic named RPY of Spherial Motor
*/
#include <ros/ros.h>
#include <serial/serial.h>
#include <std_msgs/String.h>
#include "serial_pack/compareAtt.h"
typedef union{
    float  data;
    unsigned char data8[4];
}data_u;
data_u mlx_pit;
data_u mlx_rol;
data_u mlx_yaw;
data_u imu_pit;
data_u imu_rol;
data_u imu_yaw;
typedef struct{
    float pit1;
    float pit2;
    float rol1;
    float rol2;
    float yaw1;
    float yaw2;
}compare_att;
//declare serial object
serial::Serial ser;
#define sBUFFER_SIZE 1024
#define rBUFFER_SIZE 1024
unsigned char s_buffer[sBUFFER_SIZE];
unsigned char r_buffer[rBUFFER_SIZE];
void write_callback(){

}
int main(int argc,char** argv){
    //init node
    ros::init(argc,argv,"stm32_publisher");
    //declare node handler
    ros::NodeHandle nh;
    //ros::Subscriber write_sub = nh.subscribe("write",1000,write_callback);
    //ros::Publisher turtle_vel_pub = n.advertise<geometry_msgs::Twist>("/turtle1/cmd_vel", 10);
    ros::Publisher compare_att_pub = nh.advertise<serial_pack::compareAtt>("compare_att",10);
    try{
        ser.setPort("/dev/ttyUSB0");
        if(ser.isOpen()){
			ROS_INFO_STREAM("Serial has been open!close it now!\n");
			ser.close();
		}
		else{
			ROS_INFO_STREAM("Serial has been closed!setting it now\n");
		}
        ser.setBaudrate(460800);
		ROS_INFO_STREAM("setBautrate succeed!\n");
        serial::Timeout to = serial::Timeout::simpleTimeout(1000);
        ser.setTimeout(to);
		ROS_INFO_STREAM("setTimeout succeed!\n");
        ser.open();
    }
    catch(serial::IOException){
        ROS_ERROR_STREAM("Unable to open port");
        return -1;
    }
    if(ser.isOpen()){
        ROS_INFO_STREAM("Serial Port initialized");
    }
    else{
        return -1;
    }
    ros::Rate loop_rate(50);
    while(ros::ok()){
        if(ser.available()){
            ROS_INFO_STREAM("Reading from serial port\n");
            size_t bytes_read =  ser.read(r_buffer,ser.available());
            unsigned char buffer[24] = {0};
            int state = 0;
            for(int i=0;i<bytes_read;++i){
               if(state == 0){
                   if(r_buffer[i] == 0x03){
                       state = 1;
                   }
               }
               else if(state == 1){
                   if(r_buffer[i] == 0xfc){
                       state = 2;
                   }
                   else{
                       state = 0;
                   }
               }//else if
                else if(state >=2){
                    buffer[state-2] = r_buffer[i];
                    ++state;
                    if(state==28){
                        state = 0;
                        for(int k=0;k<4;++k){
                            mlx_pit.data8[k] = buffer[k];
                            mlx_rol.data8[k] = buffer[4+k];
                            mlx_yaw.data8[k] = buffer[8+k];
                            imu_pit.data8[k] = buffer[12+k];
                            imu_rol.data8[k] = buffer[16+k];
                            imu_yaw.data8[k] = buffer[20+k];
                        }
                        ROS_INFO_STREAM("RPY From mlx calculate:"<<std::endl);
                        ROS_INFO_STREAM("Pit:"<<mlx_pit.data<<"      ");
                        ROS_INFO_STREAM("Rol:"<<mlx_rol.data<<"     ");
                        ROS_INFO_STREAM("Yaw:"<<mlx_yaw.data<<"    "<<std::endl);

                        ROS_INFO_STREAM("RPY From imu calculate:"<<std::endl);
                        ROS_INFO_STREAM("Pit:"<<imu_pit.data<<"      ");
                        ROS_INFO_STREAM("Rol:"<<imu_rol.data<<"     ");
                        ROS_INFO_STREAM("Yaw:"<<imu_yaw.data<<"    "<<std::endl);
                        serial_pack::compareAtt result;
                        result.pit1 = mlx_pit.data;
                        result.pit2 = imu_pit.data;
                        result.rol1 = mlx_rol.data;
                        result.rol2 = imu_rol.data;
                        result.yaw1 = mlx_yaw.data;
                        result.yaw2 = imu_yaw.data;
                        compare_att_pub.publish(result);
                    }
                }
            }//for
            // std_msgs::String result;
            // result.data = ser.read(ser.available());
            // ROS_INFO_STREAM("Read:"<<result.data);
        }//if
    }
    ros::spinOnce();
    loop_rate.sleep();
}
